#include <Arduino.h>

// ========== 全局引脚定义 ==========
const int MOTOR1_DIR_POS = 7;   // 电机正转引脚
const int MOTOR1_DIR_NEG = 6;   // 电机反转引脚
const int MOTOR1_SENSOR_A = 3;  // 编码器A相（中断1）
const int MOTOR1_SENSOR_B = 2;  // 编码器B相（中断0）

// ========== 全局变量 ==========
volatile long encoderCount = 0; // 编码器计数值
unsigned long lastTime = 0;     // 上次测速时间
float kp = 0.1, ki = 0.001, kd = 0.001; // 调整后的PID参数（适合低分辨率编码器）
float targetRPM = 0, currentRPM = 0;
float error = 0, prevError = 0, integral = 0, derivative = 0;
const float ENCODER_RESOLUTION = 52.0; // 使用4倍频，13PPR * 4 = 52
// ========== 电机控制器类 ==========
class MotorController {
private:
    int dirPosPin;  // 正转引脚
    int dirNegPin;  // 反转引脚

public:
    // 构造函数
    MotorController(int posPin, int negPin) : dirPosPin(posPin), dirNegPin(negPin) {}
    
    // 初始化引脚
    void init() {
        pinMode(dirPosPin, OUTPUT);
        pinMode(dirNegPin, OUTPUT);
    }
    
    // 设置电机速度（-255 ~ 255）
    void setSpeed(int speed) {
        speed = constrain(speed, -255, 255);
        if (speed > 0) {
            analogWrite(dirPosPin, speed);
            analogWrite(dirNegPin, 0);
        } else if (speed < 0) {
            analogWrite(dirPosPin, 0);
            analogWrite(dirNegPin, -speed);
        } else {
            analogWrite(dirPosPin, 0);
            analogWrite(dirNegPin, 0);
        }
    }
};

// ========== 全局中断处理函数（定义在类外） ==========
void handleEncoderA() {
    bool a = digitalRead(MOTOR1_SENSOR_A);
    bool b = digitalRead(MOTOR1_SENSOR_B);
    encoderCount += (a == b) ? 1 : -1; // A相领先为正转
}

void handleEncoderB() {
    bool a = digitalRead(MOTOR1_SENSOR_A);
    bool b = digitalRead(MOTOR1_SENSOR_B);
    encoderCount += (a != b) ? 1 : -1; // B相领先为反转
}

// ========== 实例化电机控制器 ==========
MotorController motor(MOTOR1_DIR_POS, MOTOR1_DIR_NEG);

// ========== 初始化 ==========
void setup() {
    Serial.begin(115200); // 提高波特率以获取更流畅的输出
    motor.init();
    
    // 配置编码器引脚和中断
    pinMode(MOTOR1_SENSOR_A, INPUT_PULLUP);
    pinMode(MOTOR1_SENSOR_B, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(MOTOR1_SENSOR_A), handleEncoderA, CHANGE);
    attachInterrupt(digitalPinToInterrupt(MOTOR1_SENSOR_B), handleEncoderB, CHANGE);
    
    // 配置定时器1（20ms中断，50Hz频率）
    cli();
    TCCR1A = 0;
    TCCR1B = 0;
    TCNT1 = 0;
    OCR1A = 3124; // 16MHz/(64*3125)=80Hz (12.5ms周期)
    TCCR1B |= (1 << WGM12) | (1 << CS11) | (1 << CS10);
    TIMSK1 |= (1 << OCIE1A);
    sei();
    
    lastTime = millis();
}

// ========== 定时器中断 ==========
ISR(TIMER1_COMPA_vect) {
    unsigned long now = millis();
    float dt = (now - lastTime) / 1000.0;
    if (dt == 0) return;
    
    // 计算转速（使用修正后的编码器分辨率）
    static long lastCount = 0;
    long delta = encoderCount - lastCount;
    currentRPM = (delta / ENCODER_RESOLUTION) / dt * 60.0;
    lastCount = encoderCount;
    lastTime = now;
    
    // 生成目标转速（正弦波）
    targetRPM = 100 * sin(now / 2000.0); // 降低频率，方便观察
    
    // PID控制
    error = targetRPM - currentRPM;
    integral += error * dt;
    integral = constrain(integral, -1000, 1000); // 积分限幅，防止饱和
    derivative = (error - prevError) / dt;
    
    // 计算PID输出
    int output = kp * error + ki * integral + kd * derivative;
    output = constrain(output, -255, 255);
    
    motor.setSpeed(output);
    prevError = error;
    
    // 串口输出（降低输出频率，避免阻塞）
    static unsigned long lastPrint = 0;
    if (now - lastPrint > 100) {
        Serial.print("Target: "); Serial.print(targetRPM, 1);
        Serial.print(" RPM | Current: "); Serial.print(currentRPM, 1);
        Serial.print(" | Error: "); Serial.print(error, );
        Serial.print(" | Output: "); Serial.println(output);
        lastPrint = now;
    }
}


void loop() {
    
    delay(10);
}